#ifndef BLENDMATHCLASS_H
#define BLENDMATHCLASS_H
#include "blendmath.h"

class BlendMathClass
{
public:
    BlendMathClass();


    static double findMaxTangentAngle(double v, double acc, double cycle_time);

    static double findKinkAccel(double kink_angle, double v_plan, double cycle_time);

    static double fsign(double f);

    static int clip_min(double * const x, double min);

    static int clip_max(double * const x, double max);

    static double saturate(double x, double max);

    static int sat_inplace(double * const x, double max);

    static int checkTangentAngle(PmCircle const * const circ, SphericalArc const * const arc, BlendGeom3 const * const geom, BlendParameters const * const param, double cycle_time, int at_end);

    static int findIntersectionAngle(PmCartesian const * const u1,
            PmCartesian const * const u2, double * const theta);

    static double pmCartMin(PmCartesian const * const in);

    static int calculateInscribedDiameter(PmCartesian const * const normal,
            PmCartesian const * const bounds, double * const diameter);

    static int findAccelScale(PmCartesian const * const acc,
            PmCartesian const * const bounds,
            PmCartesian * const scale);

    static int pmCartCartParallel(PmCartesian const * const v1,
            PmCartesian const * const v2, double tol);

    static int pmCircLineCoplanar(PmCircle const * const circ,
            PmCartLine const * const line, double tol);

    static int blendCalculateNormals3(BlendGeom3 * const geom);

    static int blendComputeParameters(BlendParameters * const param);

    static int blendCheckConsume(BlendParameters * const param,
            BlendPoints3 const * const points,
            TC_STRUCT const * const prev_tc, int gap_cycles);

    static int blendFindPoints3(BlendPoints3 * const points, BlendGeom3 const * const geom,
            BlendParameters const * const param);

    static int blendGeom3Init(BlendGeom3 * const geom,
            TC_STRUCT const * const prev_tc,
            TC_STRUCT const * const tc);

    static int blendParamKinematics(BlendGeom3 * const geom,
            BlendParameters * const param,
            TC_STRUCT const * const prev_tc,
            TC_STRUCT const * const tc,
            PmCartesian const * const acc_bound,
            PmCartesian const * const vel_bound,
            double maxFeedScale);

    static int blendInit3FromLineLine(BlendGeom3 * const geom, BlendParameters * const param,
            TC_STRUCT const * const prev_tc,
            TC_STRUCT const * const tc,
            PmCartesian const * const acc_bound,
            PmCartesian const * const vel_bound,
            double maxFeedScale);

    static int blendInit3FromLineArc(BlendGeom3 * const geom, BlendParameters * const param,
            TC_STRUCT const * const prev_tc,
            TC_STRUCT const * const tc,
            PmCartesian const * const acc_bound,
            PmCartesian const * const vel_bound,
            double maxFeedScale);

    static int blendInit3FromArcLine(BlendGeom3 * const geom, BlendParameters * const param,
            TC_STRUCT const * const prev_tc,
            TC_STRUCT const * const tc,
            PmCartesian const * const acc_bound,
            PmCartesian const * const vel_bound,
            double maxFeedScale);

    static int blendInit3FromArcArc(BlendGeom3 * const geom, BlendParameters * const param,
            TC_STRUCT const * const prev_tc,
            TC_STRUCT const * const tc,
            PmCartesian const * const acc_bound,
            PmCartesian const * const vel_bound,
            double maxFeedScale);

    static int blendArcArcPostProcess(BlendPoints3 * const points, BlendPoints3 const * const points_in,
            BlendParameters * const param, BlendGeom3 const * const geom,
            PmCircle const * const circ1, PmCircle const * const circ2);

    static int blendLineArcPostProcess(BlendPoints3 * const points, BlendPoints3 const * const points_in,
            BlendParameters * const param, BlendGeom3 const * const geom,
            PmCartLine const * const line1, PmCircle const * const circ2);

    static int blendArcLinePostProcess(BlendPoints3 * const points, BlendPoints3 const * const points_in,
            BlendParameters * const param, BlendGeom3 const * const geom,
            PmCircle const * const circ1, PmCartLine const * const line2);

    static int arcFromBlendPoints3(SphericalArc * const arc, BlendPoints3 const * const points,
            BlendGeom3 const * const geom, BlendParameters const * const param);

    //Not implemented yet
    static int blendGeom3Print(BlendGeom3 const * const geom);
    static int blendParamPrint(BlendParameters const * const param);
    static int blendPoints3Print(BlendPoints3 const * const points);
    static double pmCircleActualMaxVel(PmCircle * const circle,
            double v_max,
            double a_max,
            int parabolic);
    static int findSpiralArcLengthFit(PmCircle const * const circle,
            SpiralArcLengthFit * const fit);
    static int pmCircleAngleFromProgress(PmCircle const * const circle,
            SpiralArcLengthFit const * const fit,
            double progress,
            double * const angle);
    static double pmCircleEffectiveMinRadius(PmCircle const * const circle);
};

#endif // BLENDMATHCLASS_H
